%% 稳态非零参考值控制
% xd 不再是一个常数
% 质量块根据笔记本上的记录的正弦轨迹运动
%% 代码
% 定义系统
clear;
clc;
m_sys = 1;
b_sys = 0.5;
k_sys = 1;
A = [0 1;-k_sys/m_sys -b_sys/m_sys];
B = [0;1/m_sys];
C = [1 0];
D = 0;
n = size(A,1);
p = size(B,2);
%% 离散化系统
Ts = 0.1;
sys_d = c2d(ss(A,B,C,D),Ts);
A = sys_d.a;
B = sys_d.b;

%%%%%权重设计%%%%%%%%
Q = [1 0; 0 1];%n*n
S = [1 0; 0 1];%n*n
R = 0.1;%p*p
w = 0.5*pi;
%%%%%%系统初始化%%%%%%
% 参考值初始化

% xd = [1;0];
% AD = eye(n); 

%让质量块根据笔记上的要正弦轨迹运行
% 1.1
xd = [0.1;0];
AD = c2d(ss([0 1;-w^2 0],[0;0],[0 0],0),Ts).A;


x0 = [0;0];
x = x0;

u0 = 0;
u = u0;

xa = [x;xd;u];

k_steps = 200;
x_history = zeros(n,k_steps);
xa_history = zeros(5,k_steps);
u_history = zeros(p,k_steps);

[Aa,Ba,Qa,Sa,R] = F3_InputAugmentMatrix_Delta_U(A,B,Q,R,S,AD);
[F] = F1_LQR_Gain(Aa,Ba,Qa,R,Sa);
for k = 1:k_steps
    %1.2
    if (k==50)
        xd = [xd(1);-0.2];
    elseif (k==100)
        xd = [xd(1);0.2];
    elseif (k==150)
        xd = [xd(1);-0.2];
    elseif (k==200)
        xd = [xd(1);0.2];  
    end
    Delta_u = -F*xa;
    u = Delta_u + u;
    x = A * x+B * u;
    xd =AD*xd;
    xa = [x;xd;u];
    xa_history(:,k+1)=xa;
    x_history(:,k+1)=x;
    u_history(:,k)=u;  
end
%% 制图
figure; % 创建一个新的图形窗口
subplot(3,1,1);% 创建两个子图，2行1列，当前激活第一个子图
hold;
% plot(0:length(x_history)-1,x_history(2,:),'--');
plot(0:length(x_history)-1,x_history(1,:),'-');
plot(0:length(xa_history)-1,xa_history(3,:),'--');
grid on
legend("x1","x1d")%图例
% hold off;
% xlim([0 k_steps]);%设置x轴的显示氛围
% ylim([-1 2]);

% 速度与xd2比较
subplot (3, 1, 2);%切换到第二个子图
hold;
plot(0:length(x_history)-1,x_history(2,:),'b-');
plot(0:length(xa_history)-1,xa_history(4,:),'g--');
grid on
legend("x2","x2d")
hold off;
xlim([0 k_steps]);
ylim([-0.5 0.5]);


% 系统输入结果图
subplot (3, 1, 3);%切换到第二个子图
% 系统输入结果图，施加在质量块上的力，f
% stairs (0:length(u_history)-1,u_history(1,:));
plot (0:length(u_history)-1,u_history(1,:));
legend("u")
grid on
% xlim([0 k_steps]);
% ylim([-3 3]);


